

#include <sensor_common.h>
#include <sensor_bmp280.h>
#include <sensor_bmi270.h>
#include <sensor_bmi160.h>
#include <sensor_m8n.h>

#include <rtthread.h>
#include <math.h>
#include <attitude_estimate.h>

#define DBG_ENABLE
#define DBG_LEVEL DBG_LOG
#define DBG_SECTION_NAME "sensor.common"
#define DBG_COLOR
#include <rtdbg.h>

static uint32_t bmp280_baro_data;
static int32_t bmp280_temp_data;
static float uav_altitude;
static lwgps_t gps_data;

static int calc_altitude_from_baro_temp(rt_uint32_t baro, rt_uint32_t temp, float *altitude)
{
    // *altitude = (powf((101325.0f / baro), (1 / 5.257f)) - 1) * (temp / 100.0f + 273.15f) / 0.0065f;
    *altitude = 44330 * (1 - powf((baro / 101325.0f), (1 / 5.257f)));
    return 0;
}

/********************** bmp280 ***********************/
static rt_device_t bmp280_baro = RT_NULL;
static rt_device_t bmp280_temp = RT_NULL;

static int start_bmp280_sensor()
{
    int rslt = start_bmp280();
    if (rslt != RT_EOK)
        return rslt;

    // baro
    bmp280_baro = rt_device_find("bmp280_baro");
    if (bmp280_baro == RT_NULL)
    {
        LOG_E("find bmp280_baro failed\n");
        return RT_ERROR;
    }
    if (rt_device_open(bmp280_baro, RT_DEVICE_FLAG_RDWR) != RT_EOK)
    {
        LOG_E("open bmp280_baro failed\n");
        return RT_ERROR;
    }

    // temp
    bmp280_temp = rt_device_find("bmp280_temp");
    if (bmp280_temp == RT_NULL)
    {
        LOG_E("find bmp280_temp failed\n");
        return RT_ERROR;
    }
    if (rt_device_open(bmp280_temp, RT_DEVICE_FLAG_RDWR) != RT_EOK)
    {
        LOG_E("open bmp280_temp failed\n");
        return RT_ERROR;
    }

    return RT_EOK;
}

/********************** bmp280 ***********************/

/********************** bmi270 ***********************/
static rt_device_t bmi270_acce = RT_NULL;
static rt_device_t bmi270_gyro = RT_NULL;

static int start_bmi270_sensor()
{
    static rt_uint8_t bmi270_pass = 1;

    int rslt = rt_bmi270_sensor_register();
    if (rslt != RT_EOK)
    {
        rt_kprintf("regist bmi270 failed\n");
        bmi270_pass = 0;
    }

    if (bmi270_pass)
    {
        // acce
        bmi270_acce = rt_device_find(BMI270_ACCE_DEV_NAME);
        if (bmi270_acce == RT_NULL)
        {
            rt_kprintf("find bmi270_acce failed\n");
            bmi270_pass = 0;
        }
        if (rt_device_open(bmi270_acce, RT_DEVICE_FLAG_RDWR) != RT_EOK)
        {
            rt_kprintf("open bmi270_acce failed\n");
            bmi270_pass = 0;
        }

        // gyro
        bmi270_gyro = rt_device_find(BMI270_GYRO_DEV_NAME);
        if (bmi270_gyro == RT_NULL)
        {
            rt_kprintf("find bmi270_gyro failed\n");
            bmi270_pass = 0;
        }
        if (rt_device_open(bmi270_gyro, RT_DEVICE_FLAG_RDWR) != RT_EOK)
        {
            rt_kprintf("open bmi270_gyro failed\n");
            bmi270_pass = 0;
        }
    }

    return (bmi270_pass == 1) ? 0 : -1;
}

/********************** bmi270 ***********************/

/********************** bmi160 ***********************/
static rt_device_t bmi160_acce = RT_NULL;
static rt_device_t bmi160_gyro = RT_NULL;

static int start_bmi160_sensor()
{
    int rslt = start_bmi160();
    if (rslt != RT_EOK)
        return rslt;

    // acce
    bmi160_acce = rt_device_find(BMI160_ACCE_NAME);
    if (bmi160_acce == RT_NULL)
    {
        LOG_E("find %s failed\n", BMI160_ACCE_NAME);
        return RT_ERROR;
    }
    if (rt_device_open(bmi160_acce, RT_DEVICE_FLAG_RDWR) != RT_EOK)
    {
        LOG_E("open %s failed\n", BMI160_ACCE_NAME);
        return RT_ERROR;
    }

    // gyro
    bmi160_gyro = rt_device_find(BMI160_GYRO_NAME);
    if (bmi160_gyro == RT_NULL)
    {
        LOG_E("find %s failed\n", BMI160_GYRO_NAME);
        return RT_ERROR;
    }
    if (rt_device_open(bmi160_gyro, RT_DEVICE_FLAG_RDWR) != RT_EOK)
    {
        LOG_E("open %s failed\n", BMI160_GYRO_NAME);
        return RT_ERROR;
    }

    return RT_EOK;
}

/********************** bmi160 ***********************/

/********************** m8n ***********************/

static rt_device_t m8n_gps = RT_NULL;

static int start_m8n_gps()
{
    static rt_uint8_t m8n_pass = 1;

    int rslt = rt_m8n_gps_register();
    if (rslt != RT_EOK)
    {
        rt_kprintf("regist bmi270 failed\n");
        m8n_pass = 0;
    }

    if (m8n_pass)
    {
        // gps
        m8n_gps = rt_device_find(M8N_DEV_NAME);
        if (m8n_gps == RT_NULL)
        {
            rt_kprintf("find m8n gps failed\n");
            m8n_pass = 0;
        }
        if (rt_device_open(m8n_gps, RT_DEVICE_FLAG_RDWR) != RT_EOK)
        {
            rt_kprintf("open m8n gps failed\n");
            m8n_pass = 0;
        }
    }

    return (m8n_pass == 1) ? 0 : -1;
}

/********************** m8n ***********************/

static void sensor_work_thread(void *params)
{
    sensor_data_t imu_acce_data;
    sensor_data_t imu_gyro_data;

    start_bmp280_sensor();
    start_bmi160_sensor();
    start_bmi270_sensor();
    // start_m8n_gps();
    // char print_buf[0x80];

    while (1)
    {
        /* ********* bmp280计算海拔高度 ********* */
        if (bmp280_baro)
            rt_device_read(bmp280_baro, 0, &bmp280_baro_data, 1);

        if (bmp280_temp)
            rt_device_read(bmp280_temp, 0, &bmp280_temp_data, 1);

        if (bmp280_baro && bmp280_temp)
            calc_altitude_from_baro_temp(bmp280_baro_data, bmp280_temp_data, &uav_altitude);

        /* ********* bmi270更新加速度/角速度数据 ********* */
        if (bmi270_acce)
            rt_device_read(bmi270_acce, 0, &imu_acce_data, 1); /* imu_acce_data存储加速度单位是m/s^2 */

        if (bmi270_gyro)
            rt_device_read(bmi270_gyro, 0, &imu_gyro_data, 1); /* imu_gyro_data存储角速度单位是rad/s */

        if (bmi270_acce && bmi270_gyro)
            update_attitude_sensor(&imu_acce_data, &imu_gyro_data, RT_NULL);

        /* ********* bmi160更新加速度/角速度数据 ********* */
        if (bmi160_acce)
            rt_device_read(bmi160_acce, 0, &imu_acce_data, 1);

        if (bmi160_gyro)
            rt_device_read(bmi160_gyro, 0, &imu_gyro_data, 1);

        if (bmi160_acce && bmi160_gyro)
            update_attitude_sensor(&imu_acce_data, &imu_gyro_data, RT_NULL);

        if (m8n_gps)
            rt_device_read(m8n_gps, 0, &gps_data, 1);

        rt_thread_mdelay(2);
    }
}

/// @brief 开启传感器读取线程
/// @param
/// @return
int start_sensor_thread(void)
{
    rt_thread_t sensor_thread = rt_thread_create(
        "sensor_thread",
        sensor_work_thread,
        RT_NULL,
        SENSOR_THREAD_STACK_SIZE,
        SENSOR_THREAD_PRIORITY,
        SENSOR_THREAD_TICK);

    if (sensor_thread)
    {
        rt_thread_startup(sensor_thread);
    }

    return 0;
}

int get_height_value(float *height)
{
    if (height == RT_NULL)
        return RT_EINVAL;

    *height = uav_altitude;

    return RT_EOK;
}

int get_temp_value(int *temp)
{
    if (temp == RT_NULL)
        return RT_EINVAL;

    *temp = bmp280_temp_data;
    return RT_EOK;
}

int get_baro_value(uint32_t *baro)
{
    if (baro == RT_NULL)
        return RT_EINVAL;

    *baro = bmp280_baro_data;
    return RT_EOK;
}
